#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import pcl
import os

# ==============================================================================
# --- 请在这里修改您的配置 ---
# ==============================================================================

# 1. 输入的原始地图文件路径
#    请将其替换为您自己的原始PCD地图的绝对路径。
INPUT_PCD_FILE = "/home/jingzedong/B2_NAV_ws/src/hdl_Localization_ROS2/hdl_localization/data/map.pcd"

# 2. 过滤后新地图的保存路径
#    这是处理完成后，新地图保存的位置和名字。
OUTPUT_PCD_FILE = "/home/jingzedong/B2_NAV_ws/src/hdl_Localization_ROS2/hdl_localization/data/map_filtered_final.pcd"

# 3. 强度阈值 (浮点数)
#    脚本将只保留点云中强度值 大于或等于 (>=) 此值的点。
INTENSITY_THRESHOLD = 100.0

# 4. 高度阈值 (浮点数)
#    脚本将只保留点云中Z坐标 大于或等于 (>=) 此值的点。
#    常用于过滤地面以下的噪声点。
HEIGHT_THRESHOLD = -0.5

# ==============================================================================
# --- 配置结束 ---
# ==============================================================================


def filter_pcd_by_intensity_and_height(input_file, output_file, intensity_thresh, height_thresh):
    """
    使用两个直通滤波器，根据强度和高度(Z值)来过滤PCD文件。
    点云中的一个点只有在同时满足两个条件时才会被保留。
    """
    print(f"正在处理文件: {input_file}")

    # --- 1. 加载点云 ---
    try:
        cloud = pcl.load_XYZI(input_file)
        print(f"成功加载点云，包含 {cloud.size} 个点。")
    except Exception as e:
        print(f"错误: 无法加载PCD文件 '{input_file}'。")
        print(f"请确保文件路径正确，且文件包含强度(I)字段。详细错误: {e}")
        return

    # --- 2. 应用高度滤波器 (Z轴) ---
    pass_z = cloud.make_passthrough_filter()
    pass_z.set_filter_field_name("z")
    pass_z.set_filter_limits(height_thresh, float('inf'))
    cloud_after_z_filter = pass_z.filter()
    
    print(f"\n应用高度滤波器 (保留 z >= {height_thresh})...")
    print(f"  - 点数剩余: {cloud_after_z_filter.size}")

    # --- 3. 应用强度滤波器 ---
    pass_i = cloud_after_z_filter.make_passthrough_filter()
    pass_i.set_filter_field_name("intensity")
    pass_i.set_filter_limits(intensity_thresh, float('inf'))
    final_cloud = pass_i.filter()
    
    print(f"应用强度滤波器 (保留 intensity >= {intensity_thresh})...")
    print(f"  - 最终点数: {final_cloud.size}")

    # --- 4. 打印统计信息 ---
    removed_count = cloud.size - final_cloud.size
    if cloud.size > 0:
        removed_percentage = (removed_count / cloud.size) * 100
        print("\n--- 过滤总结 ---")
        print(f"  - 原始点数: {cloud.size}")
        print(f"  - 最终保留点数: {final_cloud.size}")
        print(f"  - 共移除了 {removed_count} 个点 ({removed_percentage:.2f}%)")

    # --- 5. 保存最终结果 ---
    try:
        pcl.save(final_cloud, output_file, binary=True)
        print(f"\n成功！过滤后的地图已保存至: {output_file}")
    except Exception as e:
        print(f"错误: 无法保存过滤后的PCD文件 '{output_file}'。详细错误: {e}")


if __name__ == '__main__':
    # 检查输入文件是否存在
    if not os.path.exists(INPUT_PCD_FILE):
        print(f"错误: 输入文件不存在 -> {INPUT_PCD_FILE}")
        print("请检查脚本顶部的 INPUT_PCD_FILE 路径配置是否正确。")
    else:
        # 使用脚本顶部的配置参数调用主函数
        filter_pcd_by_intensity_and_height(
            INPUT_PCD_FILE,
            OUTPUT_PCD_FILE,
            INTENSITY_THRESHOLD,
            HEIGHT_THRESHOLD
        )
